function [dLatLonAlt, t, hFig] = comparepos(latLonAltCmp, tCmp, latLonAltRef, tRef, varargin)
%COMPAREPOS 将多组位置数据与参考位置数据比较
%
% Tips:
% # latLonAltCmp/tCmp和latLonAltRef/tRef的行数可以不同，本函数仅对tCmp和tRef中均存在的时刻的位置数据进行比较
%
% Input Arguments:
% # latLonAltCmp: 长度为n的元胞数组，位置比较值，每个元素格式同latLonAltRef
% # tCmp: 长度为n的元胞数组，位置比较值对应的时刻，每个元素格式同tRef
% # latLonAltRef: 列数为3的矩阵，位置比较参考值，第1-3列分别为纬度、经度、高度，单位分别为rad、rad、m
% # tRef: 向量，位置比较参考值对应的时刻，其长度与latLonAltRef行数相同，重复元素将被剔除，单位s
% # RE: 标量，地球长半轴，单位m
% # hFig: 外部给出的figure句柄，如为NaN则不绘图，如为空则新建绘图，否则在hFig指定的绘图对象上绘图，这时plotInOneFig选项无效（默认为true），默认为空
% # cmpName: 长度为n的元胞数组，每个元素为字符串，位置比较值的名称，用于绘图，默认为'比较位置i'
% # refName: 字符串，位置参考值的名称，用于绘图，默认为'参考位置'
% # figName: 字符串，绘图名称，默认为'位置比较'
% # plotInOneFig: 逻辑标量，true表示各组比较结果绘制在一个图形上，否则各组比较结果分别绘图，默认为true
% # tol: 认为tCmp与tRef元素相等的阈值
% # fontSize: 标量，图中的字号，默认为16
%
% Output Arguments:
% # dLatLonAlt: 尺寸与latLonAltCmp相同的元胞数组，每个元素为列数为3的数组，第1-3列分别为在比较时刻的北向、东向、高度位置差值（比较值相对于参考值），单位为m
% # t: 尺寸与tCmp相同的元胞数组，每个元素为对应dLatLonAlt元素的比较时刻，单位s
% # hFig: 图形句柄
%
% Assumptions and Limitations:
% # 图例仅支持前20组（包含参考值）数据
% # 对于差值不大于0.1μs的比较时间点与参考时间点认为是同一时刻

% NOTE: 本函数使用线性索引
n = length(latLonAltCmp);

p = inputParser;
% 必须输入参数
addRequired(p, 'latLonAltCmp', @iscell);
addRequired(p, 'tCmp', @iscell);
addRequired(p, 'latLonAltRef', @(p)(isnumeric(p)&&ismatrix(p)&&(size(p, 2)==3)));
addRequired(p, 'tRef', @isnumeric);
% 可选输入参数
defaultRE = 6378137;
defaultHFig = [];
defaultCmpName = cell(size(latLonAltCmp));
for i=1:n
    defaultCmpName{i} = ['比较位置' int2str(i)];
end
defaultRefName = '参考位置';
defaultFigName = '位置比较';
defaultPlotInOneFig = true;
defaultTol = 1e-7;
defaultFontSize = 16;
addParameter(p, 'RE', defaultRE, @isnumeric);
addParameter(p, 'hFig', defaultHFig, @isnumeric);
addParameter(p, 'cmpName', defaultCmpName, @iscell);
addParameter(p, 'refName', defaultRefName, @ischar);
addParameter(p, 'figName', defaultFigName, @ischar);
addParameter(p, 'plotInOneFig', defaultPlotInOneFig, @islogical);
addParameter(p, 'tol', defaultTol, @isnumeric);
addParameter(p, 'fontSize', defaultFontSize, @isnumeric);
parse(p, latLonAltCmp, tCmp, latLonAltRef, tRef, varargin{:});

rad2Deg = 180 / pi;

dLatLonAlt = cell(size(latLonAltCmp));
t = cell(size(tCmp));
indCmp = cell(1, n);
indRef = cell(1, n);
[tRef, latLonAltRef] = makeunique(tRef, latLonAltRef);
for i=1:n
    [tCmp{i}, latLonAltCmp{i}] = makeunique(tCmp{i}, latLonAltCmp{i});
    [indCmp{1, i}, indRef{1, i}, t{i}] = findeqelem(tCmp{i}, tRef, p.Results.tol);
    dLatLonAlt{i} = posdiff(latLonAltCmp{i}(indCmp{1, i}', :), latLonAltRef(indRef{1, i}', :), p.Results.RE);
end

if isempty(p.Results.hFig) || ~isnan(p.Results.hFig)
    if ~isempty(p.Results.hFig)
        plotInOneFig = true;
    else
        plotInOneFig = p.Results.plotInOneFig;
    end
    if plotInOneFig
        totalRefIdx = [];
        for i=1:n
            totalRefIdx = horzcat(totalRefIdx, indRef{1, i}); %#ok<AGROW>
        end
        totalRefIdx = sort(unique(totalRefIdx));
    end
    colorSpecs = {'b', 'g', 'r', 'c', 'm', 'y'};
    
    for i=1:n
        if (i==1) || (~plotInOneFig)
            hFig = preparefig(p.Results.figName, p.Results.hFig);
        end
        
        hMainAxis = subplot(4, 6, [1:4 7:10 13:16]);
        if (i==1) || (~plotInOneFig)
            % 绘制基准轨迹曲线
            if plotInOneFig
                currentRefIdx = totalRefIdx;
            else
                currentRefIdx = indRef{1, i};
            end
            set(gca, 'NextPlot', 'add');
            plot3(latLonAltRef(currentRefIdx, 2)*rad2Deg, latLonAltRef(currentRefIdx, 1)*rad2Deg, latLonAltRef(currentRefIdx, 3), 'k', 'DisplayName', p.Results.refName);
            % 绘制时间标记
            idxLength = length(currentRefIdx);
            idxLengthScale = floor(log10(idxLength));
            textIdxStep = floor(idxLength / 10^(idxLengthScale)) * 10^(idxLengthScale-1);
            if textIdxStep < 1
                textIdxStep = 1;
            end
            for j=1:textIdxStep:idxLength
                text(latLonAltRef(currentRefIdx(1, j), 2)*rad2Deg, latLonAltRef(currentRefIdx(1, j), 1)*rad2Deg, latLonAltRef(currentRefIdx(1, j), 3), num2str(tRef(currentRefIdx(1, j))), 'Color', 'k');
            end
            xlabel('经度(°)');
            ylabel('纬度(°)');
            zlabel('高度(m)');
            title('轨迹曲线');
            grid on;
            view(2);
        end
        plot3(latLonAltCmp{i}(indCmp{1, i}, 2)*rad2Deg, latLonAltCmp{i}(indCmp{1, i}, 1)*rad2Deg, latLonAltCmp{i}(indCmp{1, i}, 3), colorSpecs{mod(i-1, length(colorSpecs))+1}, 'DisplayName', p.Results.cmpName{i});
        
        subplot(4, 6, [5 6]);
        if (i==1) || (~plotInOneFig)
            set(gca, 'NextPlot', 'add');
            %xlabel('时间(s)');
            title('北向位置差(m)');
            grid on;
        end
        plot(t{i}, dLatLonAlt{i}(:, 1), colorSpecs{mod(i-1, length(colorSpecs))+1});
        text(t{i}(end, 1), dLatLonAlt{i}(end, 1), num2str(dLatLonAlt{i}(end, 1), '%.0f'), 'Color', 'k', 'FontSize', p.Results.fontSize);
        
        subplot(4, 6, [11 12]);
        if (i==1) || (~plotInOneFig)
            set(gca, 'NextPlot', 'add');
            %xlabel('时间(s)');
            title('东向位置差(m)');
            grid on;
        end
        plot(t{i}, dLatLonAlt{i}(:, 2), colorSpecs{mod(i-1, length(colorSpecs))+1});
        text(t{i}(end, 1), dLatLonAlt{i}(end, 2), num2str(dLatLonAlt{i}(end, 2), '%.0f'), 'Color', 'k', 'FontSize', p.Results.fontSize);
        
        subplot(4, 6, [17 18]);
        if (i==1) || (~plotInOneFig)
            set(gca, 'NextPlot', 'add');
            % xlabel('时间(s)');
            title('高度差(m)');
            grid on;
        end
        plot(t{i}, dLatLonAlt{i}(:, 3), colorSpecs{mod(i-1, length(colorSpecs))+1});
        text(t{i}(end, 1), dLatLonAlt{i}(end, 3), num2str(dLatLonAlt{i}(end, 3), '%.0f'), 'Color', 'k', 'FontSize', p.Results.fontSize);
        
        subplot(4, 6, 19:21);
        if (i==1) || (~plotInOneFig)
            set(gca, 'NextPlot', 'add');
            xlabel('时间(s)');
            title('水平位置差和方根(m)');
            grid on;
        end
        plot(t{i}, sqrt(dLatLonAlt{i}(:, 1).^2+dLatLonAlt{i}(:, 2).^2), colorSpecs{mod(i-1, length(colorSpecs))+1});
        text(t{i}(end, 1), sqrt(dLatLonAlt{i}(end, 1).^2+dLatLonAlt{i}(end, 2).^2), num2str(sqrt(dLatLonAlt{i}(end, 1).^2+dLatLonAlt{i}(end, 2).^2), '%.0f'), 'Color', 'k', 'FontSize', p.Results.fontSize);
        
        subplot(4, 6, 22:24);
        if (i==1) || (~plotInOneFig)
            set(gca, 'NextPlot', 'add');
            xlabel('时间(s)');
            title('空间位置差和方根(m)');
            grid on;
        end
        plot(t{i}, sqrt(dLatLonAlt{i}(:, 1).^2+dLatLonAlt{i}(:, 2).^2+dLatLonAlt{i}(:, 3).^2), colorSpecs{mod(i-1, length(colorSpecs))+1});
        text(t{i}(end, 1), sqrt(dLatLonAlt{i}(end, 1).^2+dLatLonAlt{i}(end, 2).^2+dLatLonAlt{i}(end, 3).^2), num2str(sqrt(dLatLonAlt{i}(end, 1).^2+dLatLonAlt{i}(end, 2).^2+dLatLonAlt{i}(end, 3).^2), '%.0f'), 'Color', 'k', 'FontSize', p.Results.fontSize);
        
        if (i==n) || (~plotInOneFig)
            legend(hMainAxis, 'show', 'Location', 'NorthWest');
        end
    end
end
end

function [t, latLonAlt] = makeunique(t, latLonAlt)
[tUniq, ia] = unique(t);
if ~isequal(t, tUniq)
    t = tUniq;
    latLonAlt = latLonAlt(ia, :);
    warning('comparepos:notsortedorunique', 'Time vector(s) is not sorted or contains repeated element(s).');
end
end